% /* ----------------------------------------------------------------------------
%  * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
%  * Massachusetts Institute of Technology
%  * All Rights Reserved
%  * Authors: Jesus Tordesillas, et al.
%  * See LICENSE file for the license information
%  * -------------------------------------------------------------------------- */

%% RESULT for 3D for a given curve
close all; clear; clc;

addpath(genpath('./utils')); addpath(genpath('./solutions'));
interv=[-1,1];
syms t real; T3=[t*t*t t*t t 1]';

view1=30; view2=30; figure;  set(gcf, 'Position',  [500, 500, 3000, 1000])

%The given polynomial curve
P=[    0.7160   -0.1370   -0.4618    0.3999;
      0.3365    0.8679   -0.2322    0.2508;
      0.4907   -0.0914   -0.8137    0.4999];

disp("______________________________");
%%%% MINVO basis
subplot(1,3,1);hold on; plotAxesArrows(1.0);

A=getA_MV(3,interv);
pol_x=P(1,:)'; pol_y=P(2,:)'; pol_z=P(3,:)';

volumen_minvo=plot_convex_hull(pol_x,pol_y,pol_z,A,'g',0.017);
fplot3(pol_x'*T3,pol_y'*T3,pol_z'*T3,interv,'r','LineWidth',3); view(view1, view2); axis equal;

disp("The vertexes of the simplex generated by the MINVO basis are");
V=P*inv(A)
fprintf("Total volume of the simplex=%f\n", volumen_minvo)

disp("______________________________");
%%%% Bernstein basis
subplot(1,3,2); hold on; plotAxesArrows(1.0);

A=getA_Be(3,interv);

volumen_bernstein=plot_convex_hull(pol_x,pol_y,pol_z,A,'b',0.02);
fplot3(pol_x'*T3,pol_y'*T3,pol_z'*T3,interv,'r','LineWidth',3); view(view1, view2); axis equal;

disp("The vertexes of the simplex generated by the Bernstein (Bezier) basis are");
V=P*inv(A)
fprintf("Total volume of the simplex=%f\n", volumen_bernstein)

disp("______________________________");
%%%% BSpline basis
subplot(1,3,3); hold on; plotAxesArrows(1.0);
A=getA_BS(3,interv);

volumen_bspline=plot_convex_hull(pol_x,pol_y,pol_z,A,'y',0.2);
fplot3(pol_x'*T3,pol_y'*T3,pol_z'*T3,interv,'r','LineWidth',3); view(view1, view2); axis equal;

disp("The vertexes of the simplex generated by the B-Spline basis are");
V=P*inv(A)
fprintf("Total volume of the simplex=%f\n", volumen_bspline)
